/*
 * Copyright (C) 2020 1km Holding Limited
 */

#include <stdio.h>
#include <string.h>
#include <stdint.h>

#include "aos/hal/can.h"
#include "board.h"

#include "ulog/ulog.h"
#include "canbus.h"
#include "lidar_ce30.h"

#define LOG_TAG "LIDAR_CE30"

#define LIDAR_CE30_TASK_DEPTH 1024

#define F_CE30_CMD_ID0   (0x606)
#define F_CE30_CFG_ID0   (0x607)
#define F_CE30_DATA_ID0  (0x586)
#define F_CE30_HEART_ID0 (0x587)

#define F_CMD_LENGTH (5)
#define DATA_FRAME   (0)

#define F_CMD_RESERVE (0x11 << 6)
#define FRAME_RATE  (0 << 1) //20fps
#define LIDAR_OPEN  (1)
#define LIDAR_CLOSE (0)

#define OPEN_LIDAR_CMD (F_CMD_RESERVE | FRAME_RATE | LIDAR_OPEN)
#define ClOSE_LIDAR_CMD (F_CMD_RESERVE | FRAME_RATE | LIDAR_CLOSE)

#define DATA_SIZE_MAX (16)

typedef struct {
	uint32_t status:1;//1-启动状态 0-停止状态
	uint32_t ver_invalid:1;//1-版本号有效 0-版本号无效
	uint32_t error:1;//1-故障 0-正常
	uint32_t reserver:1;
	uint32_t is_central_correction:1;//1-中心校正状态 0-未中心校正状态
	uint32_t heartbeat_value:3;//心跳值，从0-7依次变化
	uint32_t ver:16;//版本号
} S_HEARTBEAT_FRAME;

typedef struct {
	uint8_t cmd;   
	/* cmd information
		bit7-6: 11
		bit5-1: 帧速率（如果为 0 或者大于 20， Lidar 就
		以 20fps 的速率运行）
		bit0：
			1—启动雷达测距采样
			0—停止雷达测距采样
	*/
	uint8_t spare; //reserved as 0x00
	uint8_t width;//宽度，单位 cm，如果为 0，则无区域限制
	uint8_t high;//深度，单位 dm，如果为 0，则无区域限制
	uint8_t expand_cmd;//目前不需要扩展命令
} S_CMD_FRAME;

typedef struct {
	uint16_t dist;//最近障碍物点距离，小端，单位cm
	uint8_t reserve0;
	int8_t angle;//最近障碍物角度，单位度
	uint8_t reserve1;
	uint8_t reserve2;
	uint8_t reserve3;
	uint8_t valid;//状态，bit0： 1-有障碍物， 0-无障碍物
} S_DATA_FRAME;

canbus_dev_t dev_ht, dev_dt; 

static int16_t lidar_distance;
static bool lidar_error_flag = 0;
static bool lidar_connect_flag = 0;

/**
 * @brief 判断雷达是否故障
 * 
 * @return bool 0表示正常，1表示雷达故障
 */
bool is_lidar_error()
{
	return lidar_error_flag;
}

/**
 * @brief 判断雷达是否失去连接
 * 
 * @return bool 0表示连接正常，1表示失去连接。
 */
bool is_lidar_lost_connect()
{
	return lidar_connect_flag;
}

/**
 * @brief 获取雷达距离
 * 
 * @return int16_t 返回雷达距离
 */
int16_t get_lidar_distance()
{
	return lidar_distance;
}

/**
 * @brief 发送雷达命令
 * 
 * @param frame 雷达命令
 * @return int32_t 0表示命令发送成功，非0则失败。
 */
static int32_t lidar_ce30_send(can_frame_t *frame)
{
	if (NULL == frame ||
		NULL == dev_ht.bus ||
		NULL == dev_ht.bus->canbus_send) {
		return -1;
	}
	return dev_ht.bus->canbus_send(frame);
}

/**
 * @brief 检测雷达的心跳包信息，检测雷达是否失去连接或雷达是否故障出错
 * 
 */
static void lidar_ce30_ht_detect()
{
	int ret = -1;
	canbus_dev_t *dev = &dev_ht;

	dev->rx_buf = (uint8_t *)aos_malloc(DATA_SIZE_MAX);
	dev->rx_id = F_CE30_HEART_ID0;
	ret = canbus_dev_register(dev);
	if (ret < 0) {
		aos_free(dev->rx_buf);
		LOGE(LOG_TAG, "dev lidarce30 register failed!");
		return ;
	}
	
	while(1){
		//lidar_ce30的心跳包时间为150ms，所以这里设置超过1s没有心跳包则认为雷达连接断开
		if (aos_sem_wait((aos_sem_t *)&dev->sem_t, 1000) != 0){
			lidar_connect_flag = 1;
			continue;
		}

		S_HEARTBEAT_FRAME * pheartbeat = (S_HEARTBEAT_FRAME *)dev->rx_buf;
		//printf("lidar ht normbal \n");

		if(pheartbeat->error)
		{
			lidar_error_flag = 1;
		}
	}
}

/**
 * @brief 检测雷达的距离数据
 * 
 */
static void lidar_ce30_data_detect()
{
	int ret = -1;
	canbus_dev_t *dev = &dev_dt;

	dev->rx_buf = (uint8_t *)aos_malloc(DATA_SIZE_MAX);
	dev->rx_id = F_CE30_DATA_ID0;

	ret = canbus_dev_register(dev);
	if (ret < 0) {
		aos_free(dev->rx_buf);
		LOGE(LOG_TAG, "dev lidarce30 register failed!");
		return ;
	}
	
	while(1){
		if (aos_sem_wait((aos_sem_t *)&dev->sem_t, 1000) != 0){
			lidar_distance = 0;
			continue;
		}
		
		S_DATA_FRAME *pdata = (S_DATA_FRAME *)dev->rx_buf;
		//printf("lidar data normbal \n");

		lidar_distance = pdata->dist;
	}
}

/**
 * @brief 关闭雷达
 * 
 * @return int32_t int32_t 0表示关闭雷达指令发送成功，非0表示失败。
 */
int32_t lidar_close()
{
	can_frame_t start_frame;
	S_CMD_FRAME start_cmd_t;

	start_frame.header.id = F_CE30_CMD_ID0;
	start_frame.header.dlc = F_CMD_LENGTH;
	start_frame.header.rtr = DATA_FRAME;

	start_cmd_t.cmd = ClOSE_LIDAR_CMD;
	start_cmd_t.spare = 0;
	start_cmd_t.high = 0;
	start_cmd_t.width = 0;
	start_cmd_t.expand_cmd = 0;

	memcpy(start_frame.data, start_cmd_t.cmd, start_frame.header.dlc);

	return lidar_ce30_send(&start_frame);
}

/**
 * @brief 打开雷达
 * 
 * @return int32_t 0表示打开雷达指令发送成功，非0表示失败。
 */
int32_t lidar_open()
{
	can_frame_t start_frame;
	S_CMD_FRAME start_cmd_t;

	start_frame.header.id = F_CE30_CMD_ID0;
	start_frame.header.dlc = F_CMD_LENGTH;
	start_frame.header.rtr = DATA_FRAME;

	start_cmd_t.cmd = OPEN_LIDAR_CMD;
	start_cmd_t.spare = 0;
	start_cmd_t.high = 0x23;
	start_cmd_t.width = 0x28;
	start_cmd_t.expand_cmd = 0;

	memcpy(start_frame.data, &start_cmd_t, start_frame.header.dlc);

	return lidar_ce30_send(&start_frame);
}

/**
 * @brief 雷达是否已经打开，仅仅在雷达扫描到车时可以作为判断。
 * 
 * @return int32_t 0表示雷达已经打开，非0则表示雷达没打开
 */
int32_t is_lidar_open()
{
	int16_t distance;
	
	distance = get_lidar_distance();
	if(0 != distance)
	{
		return 0;
	}else{
		return -1;
	}
}

/**
 * @brief 雷达初始化
 * 
 * @return int32_t 0表示初始化成功，非0则失败
 */
int32_t lidar_init()
{
	int32_t rc = 0;
	
	rc = canbus_init();
	if(0 != rc){
		return rc;
	}

	if (0 != aos_task_new("lidar_ce30_ht", lidar_ce30_ht_detect, NULL, LIDAR_CE30_TASK_DEPTH)) {
		LOGE(LOG_TAG, "create ce30_ht_routine fail");
		return -1;
	}

	if (0 != aos_task_new("lidar_ce30_data", lidar_ce30_data_detect, NULL, LIDAR_CE30_TASK_DEPTH)) {
		LOGE(LOG_TAG, "create ce30_data_routine fail");
		return -1;
	}

	return 0;
}

